package com.numericalmethod.algoquant.model.ralph2009.draft;

import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;

import org.apache.commons.math3.linear.Array2DRowRealMatrix;
import org.apache.commons.math3.linear.LUDecomposition;
import org.apache.commons.math3.linear.RealMatrix;

/**
 * Implementation of multi-dimensional root finder using Newton's method.
 * 
 *
 */
public class SystemOfEquationsSolver {

	private List<AbstractFunction> functions;
	
	public SystemOfEquationsSolver() {
		functions=new LinkedList<AbstractFunction>();
	}
	
	/**
	 * Add an equation to be solved.
	 * @param f The left hand side of the equation. right hand side is assumed to be zero.
	 */
	public void addEquation(AbstractFunction f) {
		if (!functions.isEmpty() && functions.get(0).getAxisCount()!=f.getAxisCount()) 
			throw new IllegalArgumentException("Input function axis count mismatch");
		functions.add(f);
	}
	
	public double[] solve(double[] initialGuess, int maxIteration, double epsilon) {
		if (initialGuess.length!=functions.size())
			throw new IllegalStateException("Number of variables to be solved must equal to the number of input equations");
		
		double[] currentGuess = Arrays.copyOf(initialGuess, initialGuess.length);
		
		for (int i=0;i<maxIteration;i++) {
			
			//System.out.println("Iteration: "+i);

			double[] delta = solveOnce(currentGuess);
			
			
			if (sumOfAbsDelta(delta)<=epsilon)
				break;
			
			currentGuess=adjustDifference(currentGuess, delta);
			
		}
		
		return currentGuess;
		
	}
	
	/**
	 * 
	 * @param guess
	 * @return the delta to be adjusted to the guess
	 */
	private double[] solveOnce(double[] guess) {
		double[] inputCopy = Arrays.copyOf(guess, guess.length);
		
		Array2DRowRealMatrix jacobianMatrix = new Array2DRowRealMatrix(guess.length, guess.length);
		
		for (int row=0;row<guess.length;row++) {
			AbstractFunction currentFunction = functions.get(row);
			for (int col=0;col<guess.length;col++) {
				double partialD = currentFunction.getPartialDerviative(inputCopy, col, 1);
				
				jacobianMatrix.setEntry(row, col, partialD);
			}
		}
		
		Array2DRowRealMatrix solutionDiffFromZero = new Array2DRowRealMatrix(guess.length, 1); 
		for (int row=0;row<guess.length;row++) {
			AbstractFunction f=functions.get(row);
			double val=f.compute(inputCopy);
			if (Double.isNaN(val))
				throw new IllegalStateException("unable to compute the value from one of function");
			solutionDiffFromZero.setEntry(row, 0, -1.0*val);
		}
		
		//System.out.println("jacobianMatrix: "+jacobianMatrix);
		//System.out.println("solutionDiffFromZero: "+solutionDiffFromZero);
		
		RealMatrix jacobianMatrixInv = new LUDecomposition(jacobianMatrix).getSolver().getInverse();
		
		RealMatrix deltaColumn = jacobianMatrixInv.multiply(solutionDiffFromZero);
		
		//System.out.println("delta: "+deltaColumn);
		
		double[] result=new double[guess.length];
		for (int i=0;i<result.length;i++)
			result[i]=deltaColumn.getEntry(i, 0);
		return result;
	}

	private double sumOfAbsDelta(double[] delta) { 
		double sum=0.0;
		for (int i=0;i<delta.length;i++) {
			sum+=Math.abs(delta[i]);
		}
		return sum;
	}
	
	private double[] adjustDifference(double[] input, double[] diff) {
		double[] result=new double[input.length];
		for (int i=0;i<diff.length;i++) {
			result[i]=input[i]+diff[i];
		}
		return result;
	}
	
	
	// dummy testing program
	public static void main(String[] args) {
		
		
		AbstractFunction func1=new AbstractFunction() {
			public int getAxisCount() {
				return 3;
			}
			
			public double compute(double[] input) {			
				double x=input[0];
				double y=input[1];
				double z=input[2];
				return Math.exp(-2.0*x*x*x)+4.0*y*y+z-1.0;
			}
		};
		
		AbstractFunction func2=new AbstractFunction() {
			public int getAxisCount() {
				return 3;
			}
			
			public double compute(double[] input) {
				double x=input[0];
				double y=input[1];
				double z=input[2];
				return z*Math.cos(18.0*y-14.0*x*x);
			}
		};
		
		AbstractFunction func3=new AbstractFunction() {
			public int getAxisCount() {
				return 3;
			}
			public double compute(double[] input) {
				double x=input[0];
				double y=input[1];
				double z=input[2];
				return x*y-10.0*z*y*y;
			}
		};
		
		
		SystemOfEquationsSolver s=new SystemOfEquationsSolver();
		s.addEquation(func1);
		s.addEquation(func2);
		s.addEquation(func3);
		double epsilon=0.0000000000001;
		double[] solution=s.solve(new double[] { 0.3, -4.0, 0.9 }, 1000000, epsilon);
		
		System.out.println("solution: "+solution[0]+", "+solution[1]+", "+solution[2]);
		System.out.println("test zero func1: "+func1.compute(solution));
		System.out.println("test zero func2: "+func2.compute(solution));
		System.out.println("test zero func3: "+func3.compute(solution));
	}
	
}
